using System;
using Microsoft.SPOT;

namespace WayPointNavigator
{
    public class GpsPoint  
    {
        private const double _earthRadiusInKilometers = 6367.0;
        
        public GpsPoint() { }

        public GpsPoint(double latitude, double longitude)
            : this()
        {
            Latitude = latitude;
            Longitude = longitude;
        }

        public GpsPoint(string xml)
            : this(
                Convert.ToDouble(xml.Split(';')[0]), Convert.ToDouble(xml.Split(';')[1]))
        {}
        
        public double Latitude { get; set; }  
        public double Longitude { get; set; }  

        public bool Equals(GpsPoint other)
        {
            return other.Longitude == Longitude && other.Latitude == Latitude;
        }
        public double Distance(GpsPoint to)
        {
            return DistanceInKilometers(to.Latitude, to.Longitude);
        }
        public override bool Equals(object obj)
        {
            if (ReferenceEquals(null, obj))
                return false;
            if (obj.GetType() != typeof(GpsPoint))
                return false;
            return Equals((GpsPoint)obj);
        }
        public override int GetHashCode()
        {
            unchecked
            {
                int result = Longitude.GetHashCode();
                result = (result * 397) ^ Latitude.GetHashCode();
                return result;
            }
        }

        public static bool operator ==(GpsPoint left, GpsPoint right)
        {
            return left.Equals(right);
        }
        public static bool operator !=(GpsPoint left, GpsPoint right)
        {
            return !left.Equals(right);
        }

        private double DistanceInKilometers(double lat2, double lng2)
        {
            return Distance(Latitude, Longitude, lat2, lng2, _earthRadiusInKilometers);
        }
        private double Distance(double lat1, double lng1, double lat2, double lng2, double radius)
        {
            var lat = MathMF.ToRadians(lat2 - lat1);
            var lng = MathMF.ToRadians(lng2 - lng1);
            var sinLat = MathMF.Sin(0.5 * lat);
            var sinLng = MathMF.Sin(0.5 * lng);
            var cosLat1 = MathMF.Cos(MathMF.ToRadians(lat1));
            var cosLat2 = MathMF.Cos(MathMF.ToRadians(lat2));
            var h1 = sinLat * sinLat + cosLat1 * cosLat2 * sinLng * sinLng;
            var h2 = MathMF.Sqrt(h1);
            var h3 = 2 * MathMF.Asin(MathMF.Min(1, h2));
            return radius * h3;
        }
    }
}
